#include "includes.h"
#include "drivers.h"
#include "process.h"
#include "vehicle.h"
#include "gps.h"
#include "flash_table.h"
#include "l206.h"

#define PROCESS_INTERVAL                        40
#define PROCESS_RPM_INTERVAL                    10
#define SILENT_DURATION_TIMEOUT                 300     //180s
#define RPM_FAILED_TOLENT                       10

uint8_t mHasGsensor;

OS_TCB ProcessTaskTCB;
CPU_STK PROCESS_TASK_STK[PROCESS_TASK_STK_SIZE];

OS_TCB ProcessRpmTaskTCB;
CPU_STK PROCESS_RPM_TASK_STK[PROCESS_RPM_TASK_STK_SIZE];

uint8_t mSilentFlag = FALSE;
uint8_t mEngineOn = FALSE;

uint8_t mLock = FALSE;

uint8_t mRpmFailedCount = 0;
uint8_t mChanged = FALSE;

uint8_t getImmoChanged(void)
{
    return mChanged;
}

void set_lock(void)
{
    mLock = TRUE;
    iFlash_set_immo(1);
}

void set_unlock(void)
{
    mLock = FALSE;
    iFlash_set_immo(0);
}

void process_init(void)
{
    uint8_t ret;
    OS_ERR err;
    uint8_t status;

    //set immo
    if(iFlash_get_restart_flag() == FALSE) {
        logi("restart flag = false, set lock");
        status = iFlash_get_immo();
        if(status == FALSE) {
            mChanged = TRUE;
        }
        //set_lock();
    } else {
        //clear sw restart flag
        iFlash_set_restart_flag(FALSE);
        logi("restart flag = true, clear flag");
    }
#ifdef GSENSOR_ENABLE
    logi("Initialize gsensor");
    mHasGsensor = gsensor_init();
    logi("Initialize gsensor done");
#endif
#if 1
    OSTaskCreate(
            (OS_TCB     *) &ProcessTaskTCB,
            (CPU_CHAR   *) "Process Task",
            (OS_TASK_PTR ) process_task,
            (void       *) 0,
            (OS_PRIO     ) PROCESS_TASK_PRIO,
            (CPU_STK    *) &PROCESS_TASK_STK[0],
            (CPU_STK_SIZE) PROCESS_TASK_STK_SIZE / 10,
            (CPU_STK_SIZE) PROCESS_TASK_STK_SIZE,
            (OS_MSG_QTY  ) 0,
            (OS_TICK     ) 0,
            (void       *) 0,
            (OS_OPT      ) OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR,
            (OS_ERR     *) &err
            );
    OSTaskCreate(
            (OS_TCB     *) &ProcessRpmTaskTCB,
            (CPU_CHAR   *) "Process Rpm Task",
            (OS_TASK_PTR ) process_rpm_task,
            (void       *) 0,
            (OS_PRIO     ) PROCESS_RPM_TASK_PRIO,
            (CPU_STK    *) &PROCESS_RPM_TASK_STK[0],
            (CPU_STK_SIZE) PROCESS_RPM_TASK_STK_SIZE / 10,
            (CPU_STK_SIZE) PROCESS_RPM_TASK_STK_SIZE,
            (OS_MSG_QTY  ) 0,
            (OS_TICK     ) 0,
            (void       *) 0,
            (OS_OPT      ) OS_OPT_TASK_STK_CHK | OS_OPT_TASK_STK_CLR,
            (OS_ERR     *) &err
            );
#endif
    status = iFlash_get_immo();
    logi("success immo status: %d", status);
    if(status == 1)
        set_unlock();
    else
        set_unlock();

    mRpmFailedCount = 0;
}

void process_task(void *unused)
{
//    uint32_t silent_count = 0;
    uint32_t task_count = 0;
//    uint32_t task_count_10s = 0;

//    uint8_t status;
//    uint8_t on;


#ifdef RF_MODULE_ENABLE
    rf_module_poweron();
#endif
    while(1) {
        os_delay_ms(PROCESS_INTERVAL);
        if(++task_count > (200 / PROCESS_INTERVAL)) {
            task_count = 0;
            process_500ms_hook();
        }
#if 0
        if(++task_count_10s > (4000 / PROCESS_INTERVAL)) {
            task_count_10s = 0;
            process_10s_hook();
        } 
        status = detect_motion();
        if(status) {
            logi("crash~~");
            silent_count = 0;
            if(mSilentFlag) {
                mSilentFlag = FALSE;
            }
            //delay 2 seconds for engine on
            os_delay(2);
            on = vehicle_check_engine();
            if(on) {
                mEngineOn = TRUE;
                vehicle_set_engine_on(TRUE);
                //if vehicle is gm, poweron the rf
                //if vehicle is toyota, poweroff the rf
                if(vehicle_get_type() == VEHICLE_GM) {
#ifdef RF_MODULE_ENABLE
                    rf_module_poweron();
#endif
                } else if(vehicle_get_type() == VEHICLE_TOYOTA ||
                        vehicle_get_type() == VEHICLE_EOBD) {
#ifdef RF_MODULE_ENABLE
                    rf_module_poweroff();
#endif
                }
                logi("engine ON");
            } else {
                mEngineOn = FALSE;
                vehicle_set_engine_on(FALSE);
#ifdef RF_MODULE_ENABLE
                rf_module_poweron();
#endif
                logi("engine OFF");
            }
        } else {
            if(mSilentFlag)
                continue;
            if(++silent_count >
                    SILENT_DURATION_TIMEOUT * 1000 / PROCESS_INTERVAL) {
                logi("silent~~");
                mSilentFlag = TRUE;
                //delay 2 seconds for engine on
                os_delay(2);
                on = vehicle_check_engine();
                if(on) {
                    mEngineOn = TRUE;
                    vehicle_set_engine_on(TRUE);
                    //if vehicle is gm, poweron the rf
                    //if vehicle is toyota, poweroff the rf
                    if(vehicle_get_type() == VEHICLE_GM) {
#ifdef RF_MODULE_ENABLE
                        rf_module_poweron();
#endif
                    } else if(vehicle_get_type() == VEHICLE_TOYOTA ||
                            vehicle_get_type() == VEHICLE_EOBD) {
#ifdef RF_MODULE_ENABLE
                        rf_module_poweroff();
#endif
                    }
                    logi("engine ON");
                } else {
                    mEngineOn = FALSE;
                    vehicle_set_engine_on(FALSE);
#ifdef RF_MODULE_ENABLE
                    rf_module_poweroff();
#endif
                    logi("engine OFF");
                }
            }
        }
#endif
    }
}

void process_rpm_task(void *unused)
{
    while(1) {
        process_10s_hook();
        os_delay(PROCESS_RPM_INTERVAL);
    }
}

void process_1s_hook(void)
{
}

void process_500ms_hook(void)
{
#ifdef RF_MODULE_ENABLE
    if(mLock) {
        rf_lock();
        //        logi("######%s: lock", __func__);
    } else {
        rf_unlock();
        //        logi("######%s: unlock", __func__);
    }
#endif
}

void process_10s_hook(void)
{
    uint16_t rpm = 0x0000;

    vehicle_get_rpm(&rpm);
    logi("++%s: rpm = %d, immo status = %d++", __func__, rpm, mLock);
    if(rpm == 0) {
        mRpmFailedCount ++;
        if(mRpmFailedCount >= RPM_FAILED_TOLENT) {
            loge("Engine is poweroff!!!!");
            mEngineOn = FALSE;
            //power off gps
            gps_standby(1);
        }
    } else {
        mRpmFailedCount = 0;
        mEngineOn = TRUE;
        gps_standby(0);
        if(is_suspend()) {
            //re-connect
            logi("go reboot!");
            software_reboot();
        }
    }
}

uint8_t process_check_engine(void)
{
    return mEngineOn;
}

uint8_t gsensor_init(void)
{
    uint8_t ret = FALSE;

#if uCOS_EN == 1 
    CPU_SR_ALLOC();
#endif
    DISABLE_INT();
    MPU6050_Initialize();
    if(MPU6050_TestConnection()) {
        logi("Connect MPU6050");
        MPU6050_SetMotionIntEnable(1);
        MPU6050_SetReadyIntEnable(0);
        MPU6050_SetMotDetThr(1);
        MPU6050_SetMotDetDur(2);
        ret = TRUE;
    } else {
        loge("Disconnect MPU6050");
        ret = FALSE;
    }
    ENABLE_INT();

    return ret;
}

uint8_t detect_motion(void)
{
    uint8_t status;

#if uCOS_EN == 1 
    CPU_SR_ALLOC();
#endif
    DISABLE_INT();
    status = MPU6050_GetMotDetStatus();
    ENABLE_INT();

    if(status > 0)
        return TRUE;
    else
        return FALSE;
}
